#ifndef HEXAPOD_WAVEGENERATOR
#define HEXAPOD_WAVEGENERATOR

#include <stdbool.h>
#include <stdint.h>
#include "HexapodLeg.h"

enum WaveStatus{
    STANCE_ALL,
    SWING_ALL,
    WAVE_ALL
};

/* generate linear wave [0, 1] */
typedef struct _WaveGenerator {
	float startT;              // 开始时间
	float period;              // 周期
	float stRatio;             // 触地系数
	float bias[LEG_CNT];       // 相位偏移
	float phase[LEG_CNT];      // 相位
	float phasePast[LEG_CNT];  // 相位
	uint8_t contact[LEG_CNT];      // 1 触地 0 悬空
	uint8_t contactPast[LEG_CNT];  // 1 触地 0 悬空
	uint8_t switchStatus[LEG_CNT]; // 1: switching, 0: do not switch
  enum  WaveStatus status;
	enum  WaveStatus statusPast;
	
	void (*calcContactPhase)(struct _WaveGenerator *wave, float *phaseResult, uint8_t *contactResult, enum WaveStatus status);
	float (*getTstance)(struct _WaveGenerator *wave);
	float (*getTswing)(struct _WaveGenerator *wave);
}WaveGenerator;

void waveGeneratorInit(WaveGenerator* wave, float period, float stancePhaseRatio, float *bias);

void waveGeneratorCalcWave(WaveGenerator* wave, float *phase, uint8_t *contact, enum WaveStatus status);

void waveGeneratorCalcContactPhase(WaveGenerator* wave, float *phaseResult, uint8_t *contactResult, enum WaveStatus status);

float waveGeneratorGetTstance(WaveGenerator* wave);

float waveGeneratorGetTswing(WaveGenerator* wave);

uint8_t calSwitchStatusSum(WaveGenerator* wave);



#endif